package com.guazi.trackingtest.cal.op;

import com.guazi.trackingtest.cal.data.Point3D;

public class CalculateGyr {
    public static Point3D update(Point3D preRotation, Point3D preRotationRate, Point3D currentRotationRate, double interval) {
        double alpha = preRotation.x + 0.5 * interval * (preRotationRate.x + currentRotationRate.x);
        double beta = preRotation.y + 0.5 * interval * (preRotationRate.y + currentRotationRate.y);
        double gama = preRotation.z + 0.5 * interval * (preRotationRate.z + currentRotationRate.z);

        return new Point3D(alpha, beta, gama);
    }
}

